#!/usr/bin/evn roseus

(setq ros::*compile-message* t) ;; switch for compile
(ros::load-ros-manifest "jskpointcloud")
(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(ros::load-ros-manifest "body_tracking")

#-:rbrain-basic (rbrain)

;;(setq *hmu* 12.0) ;; day
(setq *hmu* 15.0)
(setq *hvar* 45.0)

;;(setq *smu* 0.37) ;; day
;;(setq *svar* 0.08) ;; day
(setq *smu* 0.50) ;; night
(setq *svar* 0.15)
;;(setq *smu* 0.35) ;; night
;;(setq *svar* 0.30)

;;(setq *imu* 0.64) ;; day time
(setq *imu* 0.70) ;; night
(setq *ivar* 0.40)

(setq *threshold* 0.004)

(setq *hand-size* 40)

(setq *prev-hlst* nil)
(setq *first-person* nil)

(setq *base-cds*
      #s(coordinates plist nil rot #2f((-0.011918 -0.236504 0.971557) (-0.999814 -0.011918 -0.015166) (0.015166 -0.971557 -0.236318)) pos #f(0.0 0.0 1322.81)))
(setq *cntr* 0)

(defun data-cb (msg)
  (let ((hd (send msg :header))
        ;;(pts (send msg :points))
        (pts (send msg :point))
        ;;(bds (send (send msg :body_array) :array))
        (bds (send (send msg :body_array) :bodies))
        (idx (send (send msg :indices_array) :array))
        (cnt (* (expt (sqrt (* 2 pi)) 3) *hvar* *svar* *ivar*))
        3dp plst hlst blst fp-idx)

    (format t "~%~%callback ~D / ~A~%" (incf *cntr*)
            ;;(send hd :stamp)
            (send (send hd :stamp) :sec-nsec)
            )

    (setq 3dp (make-3dpointcloud-from-msg2 pts))
    (setq *tmp-3dp* 3dp)
    ;; hand coords validation
    (dolist (b bds)
      (let ((cl (send b :coords_list)))
        (push (if (> (length cl) 14)
                  (cons (ros::tf-pose->coords (elt cl 5))
                        (ros::tf-pose->coords (elt cl 8)))) hlst)))
    (setq hlst (nreverse hlst))

    ;; for viewer
    (setq axlst
          (mapcar #'(lambda (x)
                      (let ((a (make-arrow))
                            (b (make-arrow)))
                        (send a :transform (car x))
                        (send b :transform (cdr x))
                        (list a b))) hlst))

    ;; body coords
    (dolist (b bds)
      (let ((cl (send b :coords_list)))
        (push (if (> (length cl) 14)
                  (list (ros::tf-pose->coords (elt cl 0))
                        (ros::tf-pose->coords (elt cl 1))
                        (ros::tf-pose->coords (elt cl 2)))) blst)))
    (setq blst (nreverse blst))

    ;; extract user points
    (dolist (i idx)
      (push (send 3dp :filter-with-indices
                  (coerce (send i :indices) cons) :create t)
            plst))
    (setq plst (nreverse plst))
    (setq *plst* plst)

    ;; remove nan
    ;;(send *tmp-3dp* :nfilter :key #'(lambda (x) (not (isnan (elt x 0)))))

    ;; extract skin points
    (setq hplst
          (mapcar #'(lambda (pp)
                      (send pp :nfilter :ckey
                            #'(lambda (c)
                                (let* ((his (rgb2his (scale 255 c)))
                                       (i (elt his 1))
                                       (h (if (> (elt his 0) 180.0) (- (elt his 0) 360.0) (elt his 0)))
                                       (s (elt his 2)))
                                  (>
                                   (/ (* (exp (- (expt (/ (- h *hmu*) *hvar*) 2)))
                                         (exp (- (expt (/ (- s *smu*) *svar*) 2)))
                                         (exp (- (expt (/ (- i *imu*) *ivar*) 2))))
                                      cnt)
                                   *threshold*)
                                  ))
                            :create t)) plst))
    (setq *hplst* hplst)
    (pprint (send-all hplst :size))

    ;;
    (setq ret
          (mapcar #'(lambda (h hp pp bb)
                      (let ((lp (send (car h) :worldpos))
                            (rp (send (cdr h) :worldpos))
                            (hd (send (car bb) :worldpos))
                            l r)
                        (setq l (send hp :nfilter
                                      :key #'(lambda (x) (< (distance x lp) 140))
                                      :create t))
                        (setq r (send hp :nfilter
                                      :key #'(lambda (x) (< (distance x rp) 140))
                                      :create  t))
                        (setq h (send hp :nfilter
                                      :key #'(lambda (x) (< (distance x hd) 220))
                                      :create  t))
                        ;;(list (cons l r))
                        (list
                         (+ (send l :size)
                            (send r :size)
                            (send h :size))
                         (cons
                          (if (> (send l :size) *hand-size*)
                              (send l :centroid))
                          (if (> (send r :size) *hand-size*)
                              (send r :centroid))
                          ))
                        )
                      ) hlst hplst plst blst))

    (when *first-person*
      (let ((cnt 0)
            dlst)
        (dolist (b blst)
          (push
           (cons
            (+
             (distance (elt *first-person* 0)
                       (send (elt b 0) :worldpos))
             (distance (elt *first-person* 1)
                       (send (elt b 1) :worldpos))
             (distance (elt *first-person* 2)
                       (send (elt b 2) :worldpos)))
            cnt) dlst)
          (incf cnt))
        (setq dlst (sort dlst #'(lambda (x y) (<= (car x) (car y)))))
        (pprint (list 'dlst  dlst))
        (when (> (caar dlst) 500.0)
          ;; lost first-person
          (setq *first-person* nil))
        (setq fp-idx (cdar dlst))
        ))
    (when (null *first-person*)
      (let ((lst (mapcar #'(lambda (x) (car x)) ret)))
        (setq fp-idx (position (apply #'max lst) lst))
        ))

    (setq *first-person*
          (list
           (copy-object (send (elt (elt blst fp-idx) 0) :worldpos))
           (copy-object (send (elt (elt blst fp-idx) 1) :worldpos))
           (copy-object (send (elt (elt blst fp-idx) 2) :worldpos))))

    (cond
     ((null *prev-hlst*)
      (let ((hh (cadr (elt ret fp-idx))))
        (setq *prev-hlst* hh))
      )
     (*prev-hlst*
      (let* ((hh (cadr (elt ret fp-idx)))
             (pts (elt hplst fp-idx))
             (cah (car hh))
             (cdh (cdr hh)))
        (unless cah
          (let ((pp (car *prev-hlst*))
                tmp)
            (when pp
              (setq tmp (send pts :nfilter
                              :key #'(lambda (x) (< (distance x pp) 140))
                              :create  t))
              (if (> (send tmp :size) *hand-size*)
                  (setq cah (send tmp :centroid))))
            ))
        (unless cdh
          (let ((pp (cdr *prev-hlst*))
                tmp)
            (when pp
              (setq tmp (send pts :nfilter
                              :key #'(lambda (x) (< (distance x pp) 140))
                              :create  t))
              (if (> (send tmp :size) *hand-size*)
                  (setq cdh (send tmp :centroid))))
            ))
        (setq *prev-hlst* (cons cah cdh)))
      ))

    (let ((a (car *prev-hlst*))
          (b (cdr *prev-hlst*)))
      (if a (push (let ((c (make-icosahedron 100))) (send c :translate a) (setf (get c :face-color) :yellow) c) axlst))
      (if b (push (let ((c (make-icosahedron 100))) (send c :translate b) (setf (get c :face-color) :yellow) c) axlst)))
    (objects (list *plst* axlst *hplst*) *pickview* :fix t)
    ;;(objects (list *tmp-3dp* axlst *hplst*) *pickview* :fix t)
    ))

(ros::roseus "body_subscriber")
;;(ros::subscribe "/camera/rgb/points" sensor_msgs::PointCloud2 #'pcloud-cb)
(ros::subscribe "/extractor/data" body_tracking::DataSet #'data-cb)

(pickview)
(send *pickview* :resize 1200 900)
(send *pickview* :viewpoint #f(0 0 -2000))
(send *pickview* :viewangle (list 2000 -90 -90))

(ros::rate 30)
(defun ros-spin ()
  (do-until-key
   (ros::spin-once)
   (x::window-main-one)
   (ros::sleep)
   ))
(ros-spin)
